#-------------------------------------------------------------------------------
# 模块名称:    TCPA & DCPA Model实现
# 模块功能:    实现TCPA & DCPA
#-------------------------------------------------------------------------------
# 工作原理：
# 适用范围：
# 输入：       本船信息ship1，包括：本船中心点坐标，航向，实施航速
#              他船信息ship2，包括：他船中心点坐标，航向，实时航速
# 输出：       TCPA & DCPA
# 模块作者:    Bruce
# 创建时间:    02-06-2021
# 版权所有：   (c) Youan 2021
#-------------------------------------------------------------------------------

import SimMath
import math


def calcCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry,
                lon_TS, lat_TS, head_TS, speed_TS):
    # 计算本船和目标船的DCPA和TCPA
    # DCPA单位为m
    # TCPA单位为s
    # 计算渡船与目标船间的相对距离
    Dr = SimMath.calcDistanceHaversine(lon_ferry, lat_ferry, lon_TS, lat_TS)
    # 计算相对航速与正北的夹角
    head_ferry, head_TS = map(lambda x: math.radians(90 - x), [head_ferry, head_TS])
    vx1 = speed_ferry * math.cos(head_ferry)
    vy1 = speed_ferry * math.sin(head_ferry)
    vx2 = speed_TS * math.cos(head_TS)
    vy2 = speed_TS * math.sin(head_TS)
    x = vx2 - vx1
    y = vy2 - vy1
    Cr = 90 - math.degrees(math.atan2(y, x))
    Cr = (Cr + 360) % 360
    # 计算相对速度
    # Vr = (x ** 2 + y ** 2) ** 0.5
    # Vr = math.sqrt(x ** 2 + y ** 2)
    Vr = math.hypot(x, y)
    # 计算方位角
    b = SimMath.calcTrueHeading(lon_ferry, lat_ferry, lon_TS, lat_TS)
    # 计算相对距离和相对速度之间的夹角
    θ = math.radians(Cr - b - 180)
    DCPA = Dr * math.sin(θ)
    TCPA = Dr * math.cos(θ) / (Vr + 0.000001)
    return DCPA, TCPA

T = 200
speed_own = 10
head_own = 0
X_own = -speed_own * T * math.sin(math.radians(head_own))
Y_own = -speed_own * T * math.cos(math.radians(head_own))
lon_own, lat_own = SimMath.convertionXYToLonlat2([0, 0], [X_own, Y_own])

speed_target = 12
head_target = 240
X_target = -speed_target * T * math.sin(math.radians(head_target))
Y_target = -speed_target * T * math.cos(math.radians(head_target))
lon_target, lat_target = SimMath.convertionXYToLonlat2([0, 0], [X_target, Y_target])

DCPA, TCPA = calcCPA(lon_own, lat_own, head_own, speed_own,
                lon_target, lat_target, head_target, speed_target)

print(DCPA, TCPA)